#include "board_com.h"
#include "RGB.h"
#include "beep.h"
#include "can_common.h"
#include "offline.h"
#include "robotdef.h"
#include "rtthread.h"
#include "rttypes.h"


#define LOG_TAG              "board_com"
#define LOG_LVL              LOG_LVL_DBG
#include <ulog.h>

void board_com_process(struct rt_can_msg rxmsg);
uint8_t float_to_uint8(float f);
float uint8_to_float(uint8_t u);

static board_com_t board_com;
void board_com_init(board_com_init_t board_com_init)
{
#ifndef ONE_BOARD
    // 初始化板间通讯的掉线检测
    offline_event_init(board_com_init.offline_manage_init);
    board_com.offlinemanage_event = board_com_init.offline_manage_init.event;

    //板间通讯部分常量定义
    board_com.CanMessage.txmsg.ide =RT_CAN_STDID;
    board_com.CanMessage.txmsg.rtr =RT_CAN_DTR;
    board_com.CanMessage.txmsg.len =8;
    board_com.CanMessage.can_handle = board_com_init.CanMessage.can_handle;
    #if defined (GIMBAL_BOARD)
        board_com.CanMessage.tx_id = GIMBAL_ID;
    #else
        board_com.CanMessage.tx_id = CHASSIS_ID;
    #endif

#endif 
}

void board_recv(struct rt_can_msg rxmsg)
{
#ifndef ONE_BOARD
    #if defined(HERO_MODE) || defined(ENGINEER_MODE) || defined (INFANTRY_MODE) || defined (SENTRY_MODE)
        #ifdef GIMBAL_BOARD
            //防止烧错代码，出现多个相同设备的情况
            if (rxmsg.id == GIMBAL_ID)
            {
                aRGB_led_show(LED_White);
                beep_set_tune(1000000,300000);
                LOG_W("common device find,please check board define\n");
                RT_ASSERT(0);
            }
            if (rxmsg.id == CHASSIS_ID)
            {
                offline_event_time_update(OFFLINE_BOARD_CHASSIS);
                board_com_process(rxmsg);
            }
        #endif 

        #ifdef CHASSIS_BOARD
            //防止烧错代码，出现多个相同设备的情况
            if (rxmsg.id == CHASSIS_ID)
            {
                aRGB_led_show(LED_White);
                beep_set_tune(1000000,300000);
                RT_ASSERT(0);
            }
            if (rxmsg.id == GIMBAL_ID)
            {
                offline_event_time_update(OFFLINE_BOARD_YAW);
                board_com_process(rxmsg);
            }
        #endif 
    #endif  
#endif     
    
}

void *BoardRead()
{
    #if defined(GIMBAL_BOARD)
        return &board_com.Chassis_Upload_Data;
    #else
        return &board_com.Chassis_Ctrl_Cmd;
    #endif
    return RT_NULL; // 默认返回 NULL
}

/*
   target_id 发送者的id
*/
void board_send(rt_uint32_t target_id, void *data)
{
#ifndef ONE_BOARD
    #if defined (GIMBAL_BOARD)
        Chassis_Ctrl_Cmd_s *cmd = RT_NULL;
        cmd = (Chassis_Ctrl_Cmd_s *)data; 
        board_com.CanMessage.txmsg.data[0] = ((rt_int16_t)(cmd->vx               ) >> 8) & 0xFF;      
        board_com.CanMessage.txmsg.data[1] = ((rt_int16_t)(cmd->vx               ) & 0xFF);            
        board_com.CanMessage.txmsg.data[2] = ((rt_int16_t)(cmd->vy               ) >> 8) & 0xFF;       
        board_com.CanMessage.txmsg.data[3] = ((rt_int16_t)(cmd->vy               ) & 0xFF);             
        board_com.CanMessage.txmsg.data[4] = ((rt_int16_t)(cmd->offset_angle *100) >> 8) & 0xFF;   
        board_com.CanMessage.txmsg.data[5] = ((rt_int16_t)(cmd->offset_angle *100) & 0xFF);    
        board_com.CanMessage.txmsg.data[6] = ((rt_int8_t )(cmd->wz           *10));
        board_com.CanMessage.txmsg.data[7] = cmd->chassis_mode;
    #else
        Chassis_referee_Upload_Data_s *cmd = RT_NULL;
        cmd = (Chassis_referee_Upload_Data_s *)data;
    #endif
    board_com.CanMessage.txmsg.id = target_id;
    if (*board_com.CanMessage.can_handle != RT_NULL) 
    {
        can_send(*board_com.CanMessage.can_handle,&board_com.CanMessage.txmsg);
        //rt_device_write(*board_com.CanMessage.can_handle,0,&board_com.CanMessage.txmsg,sizeof(board_com.CanMessage.txmsg));
    }
#endif 
}

void board_com_process(struct rt_can_msg rxmsg){
#ifndef ONE_BOARD
    #ifdef GIMBAL_BOARD
        if (rxmsg.id == CHASSIS_ID)
        {
            offline_event_time_update((offline_event)board_com.offlinemanage_event);
        }
    #endif 
    #ifdef CHASSIS_BOARD
        if (rxmsg.id == GIMBAL_ID)
        {
            offline_event_time_update((offline_event)board_com.offlinemanage_event);
            board_com.Chassis_Ctrl_Cmd.vx = ((rt_int16_t)(rxmsg.data[0] << 8) | rxmsg.data[1]) ;
            board_com.Chassis_Ctrl_Cmd.vy = ((rt_int16_t)(rxmsg.data[2] << 8) | rxmsg.data[3]) ;
            board_com.Chassis_Ctrl_Cmd.offset_angle = ((rt_int16_t)(rxmsg.data[4] << 8) | rxmsg.data[5]) / 100.0f;
            board_com.Chassis_Ctrl_Cmd.wz = ((rt_int8_t)(rxmsg.data[6]/10.0f));
            board_com.Chassis_Ctrl_Cmd.chassis_mode = rxmsg.data[7];
        }
    #endif 
#endif     
}


